#include "only_detect.h"

SubAndPubImg::SubAndPubImg(std::string engine_path,float conf_thres, float nms_thresh, ros::NodeHandle n) 
    : DetectNode(engine_path, conf_thres, nms_thresh), n_(n)
{ 
  n_.getParam("topic_img", topic_img_);
  n_.param<std::string>("topic_img", topic_img_, "/image_left");
  n_.getParam("topic_img_new", topic_img_new_);
  n_.param<std::string>("topic_img_new", topic_img_new_, "/detect/image");

  n_.getParam("topic_boxes", topic_boxes_);
  n_.param<std::string>("topic_boxes", topic_boxes_, "/detect/boxes");
  
  n_.getParam("frame_id", frame_id_);
  n_.param<std::string>("frame_id", frame_id_, "camera");

  n_.getParam("is_pub_detect_img", is_pub_detect_img_);
  n_.param<bool>("is_pub_detect_img", is_pub_detect_img_, 1);

  std::cout << "\n---------------" << std::endl;
  std::cout << "topic_img: " << topic_img_ << std::endl;
  std::cout << "topic_img_new: " << topic_img_new_ << std::endl;
  std::cout << "topic_boxes: " << topic_boxes_ << std::endl;

  std::cout << "frame_id: " << frame_id_ << std::endl;
  std::cout << "is_pub_detect_img: " << is_pub_detect_img_ << std::endl;
  std::cout << "---------------\n" << std::endl;
  
  image_transport::ImageTransport it_(n_);
  pub_img_ = it_.advertise(topic_img_new_, 10);
  pub_box_ = n_.advertise<detect_msgs::Boxes>(topic_boxes_, 10);

  sub_img_ = it_.subscribe(topic_img_ , 10, &SubAndPubImg::pubImg, this);
}

void SubAndPubImg::pubImg(const sensor_msgs::ImageConstPtr& msg)
{
  img_ = cv_bridge::toCvShare(msg, "bgr8")->image;
  cv::Mat draw_img = img_.clone();
  std::vector<Detection> res;
  inference(img_, res);

  msg_boxes_.Boxes.clear();
  msg_boxes_.header.frame_id = frame_id_;
  msg_boxes_.header.stamp = msg->header.stamp;
  
  for (auto it : res)
  { 
    detect_msgs::Box box;

    cv::Rect cv_box = get_rect(img_, it.bbox);

    box.class_name = class_names[it.class_id];
    box.id = it.class_id;
    // box.x = static_cast<int>(it.bbox[0]);
    // box.y = static_cast<int>(it.bbox[1]);
    // box.w = static_cast<int>(it.bbox[2]);
    // box.h = static_cast<int>(it.bbox[3]);

    box.x = static_cast<int>(cv_box.x);
    box.y = static_cast<int>(cv_box.y);
    box.w = static_cast<int>(cv_box.width);
    box.h = static_cast<int>(cv_box.height);

    box.score = it.conf;
    box.p_z = 0.;
    box.p_x = 0;
    box.p_y = 0;
    msg_boxes_.Boxes.emplace_back(box);

    std::cout << box.class_name
    << " " << box.x << " " << box.y << " " << box.w << " " << box.h
    << " " << box.score << std::endl;
  }

  pub_box_.publish(msg_boxes_);

  if(is_pub_detect_img_)
  {
    draw_bbox(img_,res);
    msg_img_ = cv_bridge::CvImage(msg->header, "bgr8", img_).toImageMsg();
    pub_img_.publish(msg_img_);
  }
  
  ROS_INFO("Detection speed %.2f fps...\n", 1000.0 / cost_time_ );
  
}